/*
  SOLID - Interference Detection Library
  Copyright (C) 1997 Gino van den Bergen

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Library General Public
  License as published by the Free Software Foundation; either
  version 2 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Library General Public License for more details.

  You should have received a copy of the GNU Library General Public
  License along with this library; if not, write to the Free
  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

  Please send remarks, questions and bug reports to gino@win.tue.nl,
  or write to:
                  Gino van den Bergen
		  Department of Mathematics and Computing Science
		  Eindhoven University of Technology
		  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
*/

#include "Transform.h"

void Transform::setIdentity() {
  pos.setValue(0, 0, 0);
  orn.setIdentity();
}

void Transform::setInverse(const Transform& t) {
  pos.setValue(-dot(t.orn[X], t.pos), 
	       -dot(t.orn[Y], t.pos), 
	       -dot(t.orn[Z], t.pos));  
  orn = t.orn.transpose();
}

Transform& Transform::operator*=(const Transform& t) {
  pos.setValue(orn.rowdot(X, t.pos) + pos[X],
	       orn.rowdot(Y, t.pos) + pos[Y],
	       orn.rowdot(Z, t.pos) + pos[Z]);
  orn *= t.orn;
  return *this;
}

void Transform::setMult(const Transform& t1, const Transform& t2) {
  pos.setValue(t1.orn.rowdot(X, t2.pos) + t1.pos[X],
	       t1.orn.rowdot(Y, t2.pos) + t1.pos[Y],
	       t1.orn.rowdot(Z, t2.pos) + t1.pos[Z]);
  orn = t1.orn * t2.orn;
}

void Transform::setInvMult(const Transform& t1, const Transform& t2) {
  Vector v = t2.pos - t1.pos;
  pos.setValue(dot(t1.orn[X], v), dot(t1.orn[Y], v), dot(t1.orn[Z], v));
  orn = multT(t1.orn, t2.orn);
}

void Transform::setMatrix(const float *m) {
  orn[X].setValue(&m[0]);
  orn[Y].setValue(&m[4]);
  orn[Z].setValue(&m[8]);
  pos.setValue(&m[12]);
}

void Transform::setMatrix(const double *m) {
  orn[X].setValue(&m[0]);
  orn[Y].setValue(&m[4]);
  orn[Z].setValue(&m[8]);
  pos.setValue(&m[12]);
}


